#include "ahrs_position.h"

static int measLastindex = 0;
static FP32 _hgtCompFiltOmega = 2.0f;
static FP32 dd_height = 0.0f;

void ahrsPositionUpdate(vector3f *AccEF, FP32 PosMeas, float dt, int measUpdate)
{
	FP32 hgt_err = 0;
	FP32 integ1_input,integ2_input,integ3_input;
	FP32 omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
	attitude.accElf.x = AccEF->x * GRATITY;
	attitude.accElf.y = AccEF->y * GRATITY;
	attitude.accElf.z = (AccEF->z - 1.0f) * GRATITY;
	
	if(measUpdate != measLastindex)// fixed height
	{
		hgt_err = (PosMeas - attitude.pos.z);
		measLastindex = measUpdate;
	}
	integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
	dd_height += integ1_input * dt;
	integ2_input = dd_height + attitude.accElf.z + hgt_err * omega2 * 3.0f;
	attitude.vel.z += integ2_input * dt;
	integ3_input = attitude.vel.z + hgt_err * _hgtCompFiltOmega * 3.0f;
	attitude.pos.z += integ3_input * dt;
}
